/**
 * @file PIDController.h
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief PID控制器类
 */

#ifndef __PID_CONTROLLER_H__
#define __PID_CONTROLLER_H__

/**
 * @brief PID参数
 */
struct PIDParam_t{
    double kp = 0; // 比例系数
    double kd = 0; // 微分系数
    double ki = 0; // 积分系数
    double maxIntegral = 0; // 积分限幅(累积误差的最大值)
    double intergralRadius = 0; // 积分半径(误差绝对值小于该值时才计算积分)
    PIDParam_t() = default;
    PIDParam_t(double kp, double kd = 0, double ki = 0,
        double maxIntegralOutput = 0, double intergralRadius = 0):
        kp(kp), kd(kd), ki(ki), maxIntegral(ki ? maxIntegralOutput / ki : 0) {}
};

/**
 * @brief PID控制器
 */
class PIDController {
public:
    void setParam(PIDParam_t param);
    void setPrecision(double precision);
    virtual double calculateBias(double target, double current);
    double getBias();
    double getOutput(double target, double current, double minOutput, double maxOutput);
    double getOutput(double target, double current, double maxOutput = 0);
    bool isFinish(double precision = -1);
    void start();
    void stop();

protected:
    PIDParam_t param; // pid参数
    bool enable = false; // 输出使能
    double precision = 1e-3; // 控制精度
    double bias = 0; // 误差
    double lastBias = 0; // 上次误差
    double integralBias = 0; // 累计误差

protected:
    double calculateOutput(double target, double current);
};

/**
 * @brief 角度PID控制器
 */
class AnglePIDController: public PIDController {
public:
    double calculateBias(double target, double current) override;
};

#endif // __PID_CONTROLLER_H__
